#include <iostream>
#include <vector>
#include <string>
#include <cmath>
#include <limits>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Eigen>

// #include "point_types.h"

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;

// // 组成平面的最少点数，如果少于这个值，判定已经没有平面了
// int minPlanePoints = 50;
// // 点云聚类，去除斜坡平面外的杂点，类间点最大距离
// double maxClusterdist = 0.13; //m
// // 拟合平面时法向量的权重
// double NormalDistanceWeight = 0.1;
// // 拟合平面时参与计算的点到面的最大距离
// double DistanceThreshold = 0.1;//m


bool ransac_plane(pcl::PointCloud<pcl::PointXYZ>::Ptr &in_cloud, double &error, Eigen::VectorXf &coef, 
		  pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_filtered,pcl::PointCloud<pcl::PointXYZ>::Ptr &ground_cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr &stone_cloud);

void euc_cluster(pcl::PointCloud< pcl::PointXYZ >::Ptr& cloud, Eigen::VectorXf& plane, const double euc_threshold, 
		 std::vector< pcl::PointCloud<pcl::PointXYZRGB> >& clustered_stones);

// PlaneAndLineCrossPoint: compute one cross point between plane and line
// [input]: plane   				the plane function coefficients: Ax+By+Cz+D=0; 
//						A = plane(0)
//						B = plane(1)
//						C = plane(2)
//						D = plane(3)
// [input]: pt  				the line function, one point and direction
// [input]: dir  				direction,|dir| = 1
// [output]:out_pt				the result point
void PlaneAndLineCrossPoint(Eigen::VectorXf &plane, Eigen::Vector3d &pt, Eigen::Vector3d &dir, Eigen::Vector3d &out_pt);

// PlaneAndPlaneCorssLine: compute the cross line between two planes
// [input]: plane1   				the plane function coefficients: Ax+By+Cz+D=0; 
//						A = plane(0)
//						B = plane(1)
//						C = plane(2)
//						D = plane(3)
// [input]: plane2  				the plane function coefficients: Ax+By+Cz+D=0;
// [output]: pt  				the line function, one point and direction
// [output]: dir				direction,|dir| = 1
void PlaneAndPlaneCorssLine(Eigen::VectorXf &plane1, Eigen::VectorXf &plane2, Eigen::Vector3d &pt, Eigen::Vector3d &dir);

// SlopeDetection: compute the slope in the input pointcloud
// [input]: cloud   				the input pointcloud
// [output]:plane   				the output slope plane function coefficients: Ax+By+Cz+D=0; 
//						A = plane(0)
//						B = plane(1)
//						C = plane(2)
//						D = plane(3)
// [output]: line_pt  				the line function, one point and direction
// [output]: line_dir				direction,|dir| = 1
// [output]: slope_angle			the detected slope angle in degree (0,90)
// [output]: slope_width			the detected slope width in m
bool SlopeDetection(PointCloud &cloud, Eigen::VectorXf &plane_coef, Eigen::Vector3d &line_pt, Eigen::Vector3d &line_dir, Eigen::Vector3d &max_point, Eigen::Vector3d &min_point, double &slope_angle, double &slope_width);

// SlopeDetection: compute the slope in the input pointcloud
// [input]: cloud   				the input pointcloud
// [output]:plane   				the output slope plane function coefficients: Ax+By+Cz+D=0; 
//						A = plane(0)
//						B = plane(1)
//						C = plane(2)
//						D = plane(3)
// [output]: line_pt  				the line function, one point and direction
// [output]: line_dir				direction,|dir| = 1
// [output]: slope_angle			the detected slope angle in degree (0,90)
// [output]: slope_width			the detected slope width in m
bool SlopeDetectionwithAll(PointCloud &all_cloud, PointCloud &front_cloud, Eigen::VectorXf &plane_coef, Eigen::Vector3d &line_pt, Eigen::Vector3d &line_dir, Eigen::Vector3d &max_point, Eigen::Vector3d &min_point, double &slope_angle, double &slope_width);
